package mskruch.gps.matching.legacy;

import java.util.ArrayList;
import java.util.Date;
import java.util.List;
import java.util.logging.Logger;

import mskruch.gps.Point;
import mskruch.gps.PointFactory;
import mskruch.gps.Position;
import mskruch.gps.util.PointsUtil;

public class TrackFinder {
    private static Logger log = Logger.getLogger(TrackFinder.class.getName());

    private static void debug(String msg) {
        log.fine(msg);
    }

    private double startTolerance = 32;
    private double endTolerance = 32;

    private double checkTolerance = 64;
    private Position templateStart;
    private Position templateEnd;

    private Position[] checkPoints;
    private Point[] source;
    private int position = 0;

    private boolean approaching = true;

    public TrackFinder(Position templateStart, Position templateEnd, Position[] checkPoints) {
        super();
        this.templateStart = templateStart;
        this.templateEnd = templateEnd;
        this.checkPoints = checkPoints;
    }
    
    public int getPosition() {
        return position;
    }

    /**
     * Looking for loops
     * 
     * @param templateStart
     * @param checkPoints
     */
    public TrackFinder(Position templateStart, Position[] checkPoints) {
        super();
        this.templateStart = templateStart;
        this.checkPoints = checkPoints;
    }

    private Point calculateNearest(Position p, int pos) {
        List<Point> points = new ArrayList<Point>();
        Point currentPoint = source[pos];
        int previous = pos - 1;
        while (previous > 0) {
            if (source[previous].distanceTo(currentPoint) > 0) {
                points.add(source[previous]);
                break;
            } else {
                previous--;
            }
        }

        points.add(currentPoint);

        int next = pos + 1;
        while (next < source.length) {
            if (source[next].distanceTo(currentPoint) > 0) {
                points.add(source[next]);
                break;
            } else {
                next++;
            }
        }

        Point s = PointFactory.newPoint(p.getLongitude(), p.getLatitude(), 0, new Date().getTime());
        return PointsUtil.nearestPolygonPoint(points.toArray(new Point[points.size()]), s);
    }

    private int checkTrack() {
        final int toCheck = checkPoints.length;
        int checked = 0;
        int current = position + 1;

        for (; current < source.length; current++) {
            final Point curr = source[current];
            final Point prev = source[current - 1];

            if (checked == 0 && (PointsUtil.appDistanceToLineSegment(prev, curr, templateStart) < startTolerance)
                    && (curr.distanceTo(templateStart) < prev.distanceTo(templateStart))) {
                /* znów zbliżamy się do startu, przed pierwszym checkpointem */
                return -1;
            }

            if (PointsUtil.appDistanceToLineSegment(prev, curr, checkPoints[checked]) < checkTolerance) {
                /* trasa przebiega w wymaganej odległości od punktu kontrolnego */
                checked++;
                if (checked == toCheck) {
                    return current; /* znaleziono ostatni punkt kontrolny, wyjazd */
                }
            }
        }
        return -1;
    }

    private Point[] extractTrack(final int potentialStart, final int potentialEnd, final Point nearestToStart,
            final Point nearestToEnd) {
        List<Point> points = new ArrayList<Point>();
        points.add(nearestToStart);
        for (int i = potentialStart; i <= potentialEnd; i++) {
            final Point curr = source[i];
            if (curr.after(nearestToStart) && nearestToEnd.after(curr)) {
                points.add(curr);
            }
        }
        points.add(nearestToEnd);
        return points.toArray(new Point[points.size()]);
    }

    private int findPotentialEnd(int lastCheckPosition, Position finish) {
        int current = lastCheckPosition;

        for (; current < source.length; current++) {
            final Point curr = source[current];
            double distance = curr.distanceTo(finish);
            if (distance < endTolerance) {
                /* jesteśmy blisko startu */
                if (current + 1 == source.length || source[current + 1].distanceTo(finish) > distance) {
                    /* jeżeli ostatni punkt lub następny już się oddala */
                    return current;
                }
            }
        }
        return -1;
    }

    private boolean findPotentialStart() {
        double previousDistanceToStart = source[position].distanceTo(templateStart);

        for (; position < source.length - 1; position++) {
            final Point curr = source[position];
            final Point next = source[position + 1];

            double distanceToStart = curr.distanceTo(templateStart);
            if (distanceToStart < startTolerance) {
                if (!approaching && previousDistanceToStart > distanceToStart) {
                    /* zaczynamy się zbliżać */
                    approaching = true;
                }
                previousDistanceToStart = distanceToStart;
                /* jesteśmy blisko startu, wjazd */
                if (approaching && next.distanceTo(templateStart) > distanceToStart) {
                    /* teraz się będziemy oddalać od punktu startu */
                    approaching = false;
                    return true;
                }
            }
        }
        return false;
    }

    public double getCheckTolerance() {
        return checkTolerance;
    }

    public double getEndTolerance() {
        return endTolerance;
    }

    public double getStartTolerance() {
        return startTolerance;
    }

    public Point[] next() {
        if (source == null) {
            throw new IllegalStateException("no source given");
        }

        while (findPotentialStart()) {
            debug("potential start found at " + position + ": " + source[position]);
            Point nearestToStart = calculateNearest(templateStart, position);

            int lastCheckPosition = checkTrack();
            if (lastCheckPosition != -1) {
                debug("check passed");
            } else {
                debug("check failed");
                /* szukaj następnego początku */
                continue;
            }

            Position finish = templateEnd; // TODO ew. uwgzlędnienie przesunięcia startu
            if (finish == null) {
                /* pętla */
                finish = nearestToStart;
            }

            int potentialEnd = findPotentialEnd(lastCheckPosition, finish);

            if (potentialEnd == -1) {
                debug("finish not found");
                continue;
            }
            Point nearestToEnd = calculateNearest(finish, potentialEnd);

            return extractTrack(position, potentialEnd, nearestToStart, nearestToEnd);
        }

        debug("no potential start found");
        return null;
    }

    public void setCheckTolerance(double checkTolerance) {
        this.checkTolerance = checkTolerance;
    }

    public void setEndTolerance(double endTolerance) {
        this.endTolerance = endTolerance;
    }

    public void setSource(Point[] source) {
        this.source = source;
        this.position = 0;
        this.approaching = true;
    }

    public void setStartTolerance(double startTolerance) {
        this.startTolerance = startTolerance;
    }

}
